// 条码检测距离：10-17cm
// 二维码检测距离：19-23cm

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/PoseStamped.h>
#include <cv_bridge/cv_bridge.h>
#include <nav_msgs/Odometry.h>
#include "opencv2/opencv.hpp"
#include <zbar.h> // zbar库 sudo apt-get install libzbar0 libzbar-dev
#include <iostream>
#include <string>
#include <vector>
#include <chrono>
#include <thread>
#include <math.h>

using namespace std;
using namespace cv;

double time_between_tolerance = 3.0; // 距离上次检测到的时间后没有检测到同样条码的最大间隔 s
double time_last = 0.0;              // 上次检测时间

double ground_truth_x_where_last_shot = 0.0;
double ground_truth_y_where_last_shot = 0.0;
double ground_truth_x = 0.0;
double ground_truth_y = 0.0;
double stick_truth_x = 0.0;
double stick_truth_y = 0.0;

std::string bar_info = "";      // 条形码信息
int is_bar_found_confirmed = 0; // CODE-128是否检测到标志
int is_bar_found_cnt = 0;       // CODE-128检测到的次数
int is_bar_found_cnt_max = 3;   // CODE-128检测到的最大次数
int barcode_count = 0;          // 条形码计数器
int barcode_count_max = 1;      // 条形码最大计数

std::string qr_info = "";      // QR-Code信息
int is_qr_found_confirmed = 0; // QR-Code是否检测到标志
int is_qr_found_cnt = 0;       // QR-Code检测到的次数
int is_qr_found_cnt_max = 3;   // QR-Code检测到的最大次数
int qr_count = 0;              // QR-Code计数器
int qr_count_max = 1;          // QR-Code最大计数

cv::Mat current_frame;
bool new_frame = false;

void imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
    try
    {
        current_frame = cv_bridge::toCvShare(msg, "bgr8")->image.clone();
        new_frame = true;
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
    }
}

void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
    ground_truth_x = msg->pose.pose.position.x + stick_truth_x;
    ground_truth_y = msg->pose.pose.position.y + stick_truth_y;
}

void stickCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    stick_truth_x = msg->pose.position.x;
    stick_truth_y = msg->pose.position.y;
}

int is_yellow_found_then_hold(cv::Mat &frame)
{
    Mat hsv, mask_yellow;
    cvtColor(frame, hsv, COLOR_BGR2HSV);
    // Range [H, S, V]: MIN - [20, 119, 108], MAX - [34, 255, 220]
    Scalar lower_yellow(20, 119, 108);
    Scalar upper_yellow(34, 255, 220);
    inRange(hsv, lower_yellow, upper_yellow, mask_yellow);

    // 寻找轮廓
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;
    findContours(mask_yellow, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

    // 寻找最大轮廓
    int maxContourIdx = -1;
    double maxContourArea = 0;
    for (int i = 0; i < contours.size(); i++)
    {
        double area = contourArea(contours[i]);
        if (area > maxContourArea)
        {
            maxContourArea = area;
            maxContourIdx = i;
        }
    }
    Moments m = moments(contours[maxContourIdx]);

    int cx = int(m.m10 / m.m00); // 重心x坐标
    int imgWidth = frame.cols;
    int leftThird = imgWidth / 8;
    int rightThird = imgWidth * 2 / 3;

    int is_yellow_found = 0; // 是否检测到黄色物体
    if (maxContourIdx >= 0 && maxContourArea > 500 &&
        cx >= leftThird && cx <= rightThird )
    {
        if (ground_truth_x_where_last_shot == 0.0 && ground_truth_y_where_last_shot == 0.0)
        {
            ground_truth_x_where_last_shot = ground_truth_x;
            ground_truth_y_where_last_shot = ground_truth_y;
            is_yellow_found = 1;
        }
        else if (abs(ground_truth_x - ground_truth_x_where_last_shot) > 1 ||
                 abs(ground_truth_y - ground_truth_y_where_last_shot) > 0.4)
        {
            ground_truth_x_where_last_shot = ground_truth_x;
            ground_truth_y_where_last_shot = ground_truth_y;
            is_yellow_found = 1;
        }
    }
    return is_yellow_found;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "qr_code_scanner");
    ros::NodeHandle nh;

    ros::Subscriber image_sub = nh.subscribe("/usb_cam/image_raw", 1, imageCallback);
    ros::Subscriber location_sub = nh.subscribe("/Odometry", 1, odomCallback);
    ros::Subscriber stick_location_sub = nh.subscribe("/stick_location", 1, stickCallback);
    ros::Publisher hold_pub = nh.advertise<geometry_msgs::PoseStamped>("/hold", 10);

    zbar::ImageScanner scanner;
    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);

    ros::Rate loop_rate(30);

    while (ros::ok())
    {
        ros::spinOnce(); // 处理回调

        // 只有在收到新帧时才处理
        if (new_frame)
        {
            new_frame = false;

            cv::Mat frame = current_frame.clone();
            if (frame.empty())
            {
                ROS_ERROR("Empty frame");
                loop_rate.sleep();
                continue;
            }

            if (1)
            {
                cv::Mat gray;
                cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);

                zbar::Image image(frame.cols, frame.rows, "Y800", gray.data, frame.cols * frame.rows);
                int n = scanner.scan(image);

                bool found = false;
                // cout << "test: " << 1 << endl;

                for (auto symbol = image.symbol_begin(); symbol != image.symbol_end(); ++symbol)
                {

                    // 获取扫描内容（字符串格式）
                    std::string data_str = symbol->get_data();
                    std::string type = symbol->get_type_name();

                    // 打印扫描内容（不论内容是什么）
                    // std::cout << "解码数据: " << data_str << std::endl;
                    // std::cout << "条码类型: " << type << std::endl;

                    /////////////////////////////////////////
                    if (type == "CODE-128" && barcode_count < barcode_count_max)
                    {
                        time_last = ros::Time::now().toSec();
                        if (is_bar_found_confirmed == 0)
                        {
                            if (is_bar_found_cnt == 0)
                            {
                                is_bar_found_cnt++;
                                bar_info = data_str;
                                ROS_INFO("条形码目标初始化：%s", bar_info.c_str());
                            }
                            else
                            {
                                if (bar_info == data_str)
                                {
                                    is_bar_found_cnt++;
                                    ROS_INFO("条形码计数 %d | %d 下", is_bar_found_cnt, is_bar_found_cnt_max);
                                    if (is_bar_found_cnt >= is_bar_found_cnt_max)
                                    {
                                        is_bar_found_confirmed = 1;
                                        bar_info = data_str;
                                        ROS_INFO("条形码确认：%s", bar_info.c_str());
                                    }
                                }
                                else
                                {
                                    is_bar_found_cnt = 0;
                                }
                            }
                        }
                        else if (is_bar_found_confirmed == 1)
                        {
                            ROS_INFO("确认条形码：%s, 第 %d 张", bar_info.c_str(), ++barcode_count);
                            is_bar_found_cnt = 0;
                            bar_info = "";
                            // 在图像上绘制矩形框
                            std::vector<cv::Point> points;
                            for (int i = 0; i < symbol->get_location_size(); i++)
                            {
                                points.push_back(cv::Point(symbol->get_location_x(i), symbol->get_location_y(i)));
                            }
                            cv::polylines(frame, points, true, cv::Scalar(0, 255, 0), 2);

                            // 在二维码上方绘制文本
                            cv::Point textPos(points[0].x, points[0].y - 10);
                            cv::putText(frame, data_str, textPos, cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 2);
                            std::string filename = "/home/firefly/temp/" + std::to_string(barcode_count) + ".jpg";
                            cv::imwrite(filename, frame);
                        }
                    }
                    if (type == "QR-Code" && qr_count < qr_count_max)
                    {
                        time_last = ros::Time::now().toSec();
                        if (is_qr_found_confirmed == 0)
                        {
                            if (is_qr_found_cnt == 0)
                            {
                                is_qr_found_cnt++;
                                qr_info = data_str;
                                ROS_INFO("二维码目标初始化：%s", qr_info.c_str());
                            }
                            else
                            {
                                if (qr_info == data_str)
                                {
                                    is_qr_found_cnt++;
                                    ROS_INFO("二维码计数 %d | %d 下", is_qr_found_cnt, is_qr_found_cnt_max);
                                    if (is_qr_found_cnt >= is_qr_found_cnt_max)
                                    {
                                        is_qr_found_confirmed = 1;
                                        qr_info = data_str;
                                        ROS_INFO("二维码确认：%s", qr_info.c_str());
                                    }
                                }
                                else
                                {
                                    is_qr_found_cnt = 0;
                                }
                            }
                        }
                        else if (is_qr_found_confirmed == 1)
                        {
                            ROS_INFO("确认二维码：%s, 第 %d 张", qr_info.c_str(), ++qr_count);
                            is_qr_found_cnt = 0;
                            qr_info = "";
                            // 在图像上绘制矩形框
                            std::vector<cv::Point> points;
                            for (int i = 0; i < symbol->get_location_size(); i++)
                            {
                                points.push_back(cv::Point(symbol->get_location_x(i), symbol->get_location_y(i)));
                            }
                            cv::polylines(frame, points, true, cv::Scalar(0, 255, 0), 2);

                            // 在二维码上方绘制文本
                            cv::Point textPos(points[0].x, points[0].y - 10);
                            cv::putText(frame, data_str, textPos, cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 2);
                            std::string filename = "/home/firefly/temp/" + std::to_string(qr_count + 3) + ".jpg";
                            cv::imwrite(filename, frame);
                        }
                    }
                    ////////////////////////////////////////////

                    // 在图像上绘制矩形框
                    std::vector<cv::Point> points;
                    for (int i = 0; i < symbol->get_location_size(); i++)
                    {
                        points.push_back(cv::Point(symbol->get_location_x(i), symbol->get_location_y(i)));
                    }
                    cv::polylines(frame, points, true, cv::Scalar(0, 255, 0), 2);

                    // 在二维码上方绘制文本
                    cv::Point textPos(points[0].x, points[0].y - 10);
                    cv::putText(frame, data_str, textPos, cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 2);
                }
            }

            int is_yellow_found = is_yellow_found_then_hold(frame);
            if (is_yellow_found)
            {
                cout << is_yellow_found << endl;      
                time_last = ros::Time::now().toSec();
            }

            if (ros::Time::now().toSec() - time_last > time_between_tolerance)
            {
                is_bar_found_cnt = 0;
                is_bar_found_confirmed = 0;
                is_qr_found_cnt = 0;
                is_qr_found_confirmed = 0;            }
            geometry_msgs::PoseStamped hold_msg;
            if ((is_bar_found_cnt && barcode_count < barcode_count_max) ||
                (is_qr_found_cnt && qr_count < qr_count_max) ||
                is_yellow_found ||
                ros::Time::now().toSec() - time_last < time_between_tolerance)
            {
                hold_msg.pose.orientation.w = 1.0;
            }
            else
            {
                hold_msg.pose.orientation.w = 0.0;
            }
            cout << "is_hold: " << hold_msg.pose.orientation.w << endl;
            hold_pub.publish(hold_msg);

            cv::imshow("Camera Feed", frame);
            if (cv::waitKey(1) == 27)
            {
                break;
            }
        }

        loop_rate.sleep();
    }

    return 0;
}